


#include <ctl_api.h>
#include <stdio.h>
#include "freeRTOS.h"
#include "task.h"
#include "Vector.h"

extern "C"
{
#include <string.h>
#include "main.h"
#include "Logomatic_V2.h"
#include "ADC.h"
//#include "i2c.h"

#if LOG_DATA
#include "fat16.h"
#include "rootdir.h"
#include "sd_raw.h"
#endif
}

#define BUFFER_LENGTH       4
#define ACCEL_THRESHOLD     50.0f
#define MIN_THRESHOLD       10.0f
#define ERROR_THRESHOLD     5.0f
#define MESG_LENGTH         6

#define TASK_STACK_SIZE             300
#define PID_TASK_PRIORITY           4
#define Timer_1ms_TASK_PRIORITY     5
#define LOGGING_TASK_PRIORITY       3
#define COUNTING_TASK_PRIORITY      2


//const unsigned long crp __attribute__ ((at(0x2FC))) = 0x87654321;


// TILT SENSOR COMMAND 
#define   MEAS            0b00000000
#define   RWTR            0b00001000
#define   RDSR            0b00001010
#define   RLOAD           0b00001011
#define   STX             0b00001110
#define   STY             0b00001111
#define   RDAX            0b00010000
#define   RDAY            0b00010001

#define PINSEL0_TXD0 0b01
#define PINSEL0_RXD0 (0b01<<2)
#define PINSEL0_TXD1 (0b01<<16)
#define PINSEL0_RXD1 (0b01<<18)

int tilt_x=1024;
int tilt_y=1024;
// spi1 
#define  spi_cs   20
#define  spi_miso 18
#define  spi_mosi 19
#define  spi_clk  17

////i2c
#define PINSEL0_SCL0 (0B01<<4)
#define PINSEL0_SDA0 (0b01<<6)
#define PINSEL0_SCL1 (0B11<<22)
#define PINSEL0_SDA1 (0b11<<28)

#define acc_add 0b00111000
#define gyro_add 0b11010000

#define OUT_X_LSB 0x05
#define OUT_X_MSB 0x06
#define OUT_Y_LSB 0x07 
#define OUT_Y_MSB 0x08
#define OUT_Z_LSB 0x09 
#define OUT_Z_MSB 0x0A
#define OUT_X_GYRO 0xA8

#define CTRL_REG1 0x38
#define ACC_XYZ_DATA_CFG 0x16

#define acc_init 0b00000001  //50hz +-2g

#define GREEN_LED 11//30
#define RED_LED 30//11

void i2c_init(void);
void i2c_acc_init(void);
void i2c_gyro_init(void);
void get_acc_data(void);
void get_gyro_data(void);
//float get_acc_z(void);
void I2CISR_handler(void);
float acc_cal_x(void);
float acc_cal_z(void);
float acc_cal_y(void);
void data_cal(void);

unsigned I2CAddress;
unsigned I2C_reg_Address;
unsigned I2CData;
#define read_gyro 1
#define read_acc 0
#define data_length 6
char i2cact=0;
char write_count=0;
char read_count=0;
char multi_read=0;
int acc_data_sum[data_length];
char acc_data_mean[data_length];
char acc_data[data_length];

char gyro_data[data_length];
char gyro_data_mean[data_length];
int gyro_data_sum[data_length];
char data_count=0;
int acc_x=0;
int acc_y=0;
int acc_z=0;
int gyro_x=0;
int gyro_y=0;
int gyro_z=0;
char i2c_bus_busy=0;

//i2c end
static int CRC(char* msg, int num);

const int TICKS_PER_REVOLUTION = 2500;    //90/26*360;    // number of ticks per revolution of the wheel  both rising and falling edge
const float WHEEL_RADIUS = 3.9;           // radius of the wheel (in inches)

const float BRAKE_PROP  = 1.04f;
const float PUSH_PROP   = 0.9f;
float speed_prop = 2.0f;
float curr_prop = 0.0f;
float prev_prop = 0.0f;
float curr_prop_sw = 0.0f;
float speed_prop_sw = 0.0f;

float _speed = 0.0f;            // current speed (initial=0)

int manual_step=40;
int manual_output=0;
int dac_output=0;
char mesg0[MESG_LENGTH];
char mesg1[MESG_LENGTH];

/***************************/
/* PID Control Variables   */
/***************************/
  float _percent_error = 0.0f;
  float error_percent[4];
  unsigned int pid_delay=0;
  unsigned int push_delay=0;
  
  float previous_error = 0.0f;
  float integral=0,derivative=0,error=0;
  float output = 0.0f;
  unsigned int duty = 0;
  unsigned int duty_stable = 0;
  bool stable=false;
  bool pip=false;

static const int HIGH_COAST_TIME=90;
static const int NORMAL_COAST_TIME=60;
static const int LOW_COAST_TIME=30;
static int COAST_TIME=NORMAL_COAST_TIME;        // defined in 10ths of a second (50=5seconds)
static int coasting_time = 0;

static const int AUTO_TIME_DELAY  = 10;         // time after releasing FSR before auto mode kicks in (defined in 10ths of a second)
int DELAY_AFTER_MANUAL = 0;

static const int MANUAL_MODE  = 1;
static const int AUTO_MODE    = 0;
static const int OFF_MODE     = 2;
static int DRIVE_MODE = MANUAL_MODE;

static const int sw_press  = 1;
static const int sw_release  = 0;
static const int sw_coast  = 2;
static const int sw_stop  = 3;

int sw_state=sw_stop;
int sw_mode=sw_release;
int sw_pre_mode= sw_release;

bool push_detected = false;
int push_detected_delay=0;
int push_start_delay=0;
int _adcval;
int pre_adcval;
int pressed=0;
int pressed_delay=0;

/***************************/
/*Push Detection Variables */
/***************************/
  unsigned int cap_val=0;
  unsigned int prev_cap_val = 0;
  signed int accel_vals[BUFFER_LENGTH];
  float filtered_vals[BUFFER_LENGTH];
  float filtered_DD[BUFFER_LENGTH];
  signed int diff[BUFFER_LENGTH];
  signed int doublediff[BUFFER_LENGTH];

  float filter_acc=0;
  float pre_acc=0;
  float pre_speed=0;
  float filter_dd=0;
  float ratio=0;
  float log_speed=0;

//butter filter
  float filter_b[5]={0.0004f,0.0017f,0.0025f,0.0017f,0.0004f};
  float filter_a[5]={1.0000f,-3.1806f,3.8612f,-2.1122f,0.4383f};
  float filter_z[5]={0,0,0,0,0};
//  float filter_b[3]={ 0.0413f,0.0825f, 0.0413f};
//  float filter_a[3]={1.0000f,   -1.3490f,    0.5140f};
//  float filter_z[3]={0,0,0};
  float filter_x=0;
  float filter_y=0;
  float filter_speed=0;

#if LOG_DATA
  struct fat16_file_struct* handle;
  char name[32];
  char _str1[50];      // used for writing to the uSD card
#endif

typedef struct
{
   uint32_t frequency;            // Blinking frequency
} sTaskParams_t, *spTaskParams_t;

typedef struct
{
   float Kp,          // proportional constant
         Ki,          // integral constant
         Kd;          // derivative constant
   float setpoint;    // setpoint for PID
   uint32_t frequency;// frequency for task
} sPIDParams_t, *spPIDParams_t;

sTaskParams_t CountingArgs = {
                            20    // Task Execution Frequency
                            };
sTaskParams_t Timer1msArgs =    {
                            1000    // Task Execution Frequency
                            };
sPIDParams_t PIDArgs =      {
                            10,   // Kp
                            6,    // Ki
                            2,    // Kd
                            0,    // initial speed setpoint
                            20    // Task Execution Frequency
                            };

extern "C"
{
  int           main              ( void );
  void          bootup            ( void );
  void          PllFeed           ( void );
  static void   initTimer1        ( void );
  void          spi_init          ( void );
  int           spi_receive_x       ( void );
  int           spi_receive_y       ( void );
  void          spi_trans         (char DATA);
  void          spi_sw_init       ( void );
  int           spi_sw_receive_x  ( void );
  int           spi_sw_receive_y  ( void );
  void          _wait_1us         ( void );
  void          uart0_tx          (char data);
  void          uart1_tx          (char data);

#if LOG_DATA
  void          card_init         ( void );
  void          log_string        ( char *buf );
#endif

}


char button_longhold=0;
char cruse=0;




xTaskHandle xCountingTaskHandle,
            xPIDTaskHandle,
            xTimer_1msHandle;
static void   CountingTask        ( void *pvParameters );
static void   PIDTask             ( void *pvParameters );
static void   Timer_1msTask       ( void *pvParameters );

#if LOG_DATA
  xTaskHandle xLoggingTaskHandle;
  sTaskParams_t LoggingArgs = {
                              10    // Task Execution Frequency
                              }; 
  static void   LoggingTask         ( void *pvParameters );
#endif

void          readSwitches        ( void );

void          vTimer1ISRWrapper   ( void ) __attribute__ ( ( naked ) );
void          vTimer1Handler      ( void ) __attribute__ ( ( noinline ) );
void          UNDEF_Routine       ( void ) __attribute__ ( ( interrupt ( "UNDEF" ) ) );

extern "C" void abort( void )
{
  while (1 == 1)
  {}
} // abort


extern "C" void bootup( void )
{
portENTER_CRITICAL();
{
    SCS=3;

    // Setting Multiplier and Divider values
    PLLCFG=0x25; // M = 6
    PllFeed();
    
    // Enabling the PLL */
    PLLCON=0x1;
    PllFeed();
    
#define PLOCK 0x400
    // Wait for the PLL to lock to set frequency
    while(!(PLLSTAT & PLOCK)) ;
    
    // Connect the PLL as the clock source
    PLLCON=0x3;
    PllFeed();
    
    // Enabling MAM and setting number of clocks used for Flash memory fetch (4 cclks in this case)
    //MAMTIM=0x3; //VCOM?
    MAMCR=0x2;
    MAMTIM=0x4; //Original
    
    // Setting peripheral Clock (pclk) to System Clock (cclk)
    VPBDIV=0x1;

    P0DIR |= (1<<STAT1)|(1<<STAT0);
    
    PINSEL1 |= (2<<18);  // AOUT is on P0.25 (logomatic P4)

    PINSEL0 = 0;
    P0DIR &= ~(3);
    P0DIR |= (1<<GREEN_LED)|(1<<RED_LED);
    P0SET =(1<<GREEN_LED)|(1<<RED_LED);
}
portEXIT_CRITICAL();
      
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
}

extern "C" void PllFeed( void )
{
    PLLFEED=0xAA;
    PLLFEED=0x55;
}

void uart0ISR (void)
{
  char data;
  switch(U0IIR& 0x0f)
  {
    case 0b0100:
    {
    data=U0RBR&0xFF;
    }
    break;

    default:
    break;
  }
}

void uart1ISR (void)
{
  char data;
  switch(U1IIR& 0x0f)
  {
    case 0b0100:
    {
    data=U1RBR&0xFF;
    }
    break;

    default:
    break;
  }
}

extern "C" void uart_init (void)
{
 PINSEL0|= PINSEL0_TXD0|PINSEL0_RXD0|PINSEL0_TXD1|PINSEL0_RXD1; 
 U0LCR |= 0B11;    //8 BIT CHAR  1 STOP BIT NO PARITY
 U0LCR |= 1<<7;    // DLAB=1
 U0DLL = 74;
 U0DLM = 0;
 U0FDR = 7 | (12<<4);   //BAUD RATE 38400

// U0DLL = 170;
// U0DLM = 1;
// U0FDR = 1 | (10<<4);   //BAUD RATE 9600
 U0LCR &= ~(1<<7);   //DLAB =0
 U0FCR |= 1|(1<<1)|(1<<2); // ENABLE AND CLEAR FIFO
 U0TER = 1<<7;  //ENABLE TRANSMIT
// U0IER |= 1<<0; //RDA INTERRUPT ENABLE

 U1LCR |= 0B11;    //8 BIT CHAR  1 STOP BIT NO PARITY
 U1LCR |= 1<<7;    // DLAB=1
 U1DLL = 74;
 U1DLM = 0;
 U1FDR = 7 | (12<<4);   //BAUD RATE 38400
// U1DLL = 170;
// U1DLM = 1;
// U1FDR = 1 | (10<<4);   //BAUD RATE 9600
 U1LCR &= ~(1<<7);   //DLAB =0
 U1FCR |= 1|(1<<1)|(1<<2); // ENABLE AND CLEAR FIFO
 U1TER = 1<<7;  //ENABLE TRANSMIT
// U1IER |= 1<<0; //RDA INTERRUPT ENABLE

 // ISR SETTINGS
// VICIntSelect &= ~( 1<<6 );  //UART0 
// VICVectCntl0 = 0x20 | 6;    //UART0
// VICVectCntl1 = 0x20 | 7;    //UART1
// VICVectAddr0 = (unsigned int)uart0ISR;
// VICVectAddr1 = (unsigned int)uart1ISR;
// VICIntEnable |= ( 1<<6 )|(1<<7);   // ISR ENABLE
}

static int CRC(char* msg,int len)
{
    int crc = 65535;
    int i,j;
    for (i=0;i<len;i++)
    {
        crc ^= msg[i];
        for (j=0;j<8;j++)
        { 
            if ( !(crc & 0x01) )
            {
                crc = crc>>1;
                crc &= 0x7FFF;
            }
            else 
            {
                crc = crc>>1;
                crc &= 0x7FFF;
                crc ^= 0xA001;
            }
        }
    }
    return crc;
}

extern "C" void uart0_tx_array (char* data, char length)
{ int i=0;
  for (i=0;i<length;i++)
    {
//    uart0_tx(0x12); 
//    uart0_tx(0x34); 
//    uart0_tx(i); 
    uart0_tx(data[i]&0xff); 
//    vTaskDelay(5);
//    uart0_tx(0x00); 
    }
}

extern "C" void uart1_tx_array (char* data, char length)
{ int i=0;
  for (i=0;i<length;i++)
    {
    uart1_tx(data[i]&0xff);
//    vTaskDelay(5);
//    _wait_1us();
//    uart1_tx(0xff); 
    }
}

extern "C" void uart0_tx (char data)
{
while(U0LSR|(1<<5)) //THR IS EMPTY
  {
  U0THR=data;
  break;
  }
}

extern "C" void uart1_tx (char data)
{
  portENTER_CRITICAL ();
while(U1LSR|(1<<5)) //THR IS EMPTY
  {
  U1THR=data;
  break;
  }
  portEXIT_CRITICAL (); 
  vTaskDelay(1);
}

extern "C" void spi_sw_init (void)
{
    /* Configure Pin Connect Block */
    //PINSEL1 |= (1<<3)|(1<<5)|(1<<7);//|(1<<9); 
    P0DIR |= (1<<spi_cs)|(1<<spi_mosi)|(1<<spi_clk); 
    P0CLR = 1<<spi_clk;
    P0SET = 1<<20;

     /* Set pclk to same as cclk */
    //VPBDIV=0x1;
    //SSPCPSR=0X20;    // CLK/64   12M/64=187.5K
    //S0SPCCR=0x8;
//    SSPCR0= 0B1111 | (31<<8)|(0<<6)|(0<<7);    //8bit  //1111=16bit
//    P0SET = 1<<20;
//    SSPCR1=1<<1;     //MSATER MODE & ENABLE SSP
    /* Device selected as master */
    //S0SPCR=0x20;
}

extern "C" void spi_init (void)
{
    /* Configure Pin Connect Block */
    PINSEL1 |= (1<<3)|(1<<5)|(1<<7);//|(1<<9); 
    P0DIR |= 1<<20; 
     /* Set pclk to same as cclk */
    //VPBDIV=0x1;
    SSPCPSR=0X20;    // CLK/64   12M/64=187.5K
    //S0SPCCR=0x8;
    SSPCR0= 0B1111 | (31<<8)|(0<<6)|(0<<7);    //8bit  //1111=16bit
    P0SET = 1<<20;
    SSPCR1=1<<1;     //MSATER MODE & ENABLE SSP
    /* Device selected as master */
    //S0SPCR=0x20;
}

extern "C" void spi_trans (char DATA)
{
    P0CLR = 1<<20;
    /* Write data out */
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
    SSPDR=(DATA<<8);
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4)))
    {}   
    P0SET = 1<<20;
}

extern "C" int spi_sw_receive_y (void)
{
    P0CLR = 1<<spi_cs;
    int _result=0;
    char _data;
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    _data=RDAY;
    int i=8;
    while(i>0)  //write 8 bit data
    {
      P0CLR = 1<<spi_clk;
      if (_data & 0x80)
        P0SET = 1<< spi_mosi;
        else
        P0CLR = 1 << spi_mosi;

      _data <<=1;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET= 1 << spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      i--;
    }
    i=11;    // read 11 bit data
    while (i>0)
    {
      P0CLR = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      _result<<=1;
      if(P0PIN & (1 << spi_miso))
      _result++;
      i--;   
    }
    P0CLR = 1<<spi_clk;
    P0SET = 1<<20;
    return _result;
}

extern "C" int spi_sw_receive_x (void)
{
    P0CLR = 1<<spi_cs;
    int _result=0;
    char _data;
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    _data=RDAX;
    int i=8;
    while(i>0)  //write 8 bit data
    {
      P0CLR = 1<<spi_clk;
      if (_data & 0x80)
        P0SET = 1<< spi_mosi;
        else
        P0CLR = 1 << spi_mosi;

      _data <<=1;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET= 1 << spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      i--;
    }
    i=11;    // read 11 bit data
    while (i>0)
    {
      P0CLR = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      _result<<=1;
      if(P0PIN & (1 << spi_miso))
      _result++;
      i--;    
    }
    P0CLR = 1<<spi_clk;
    P0SET = 1<<20;
    return _result;
}

extern "C" void _wait_1us (void)
{
  int i=0;
  for (i=0;i<10;i++)
  {}
}

extern "C" int spi_receive_x (void)
{
    P0CLR = 1<<20;
    int _result;
    int _temp,_temp1;
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    SSPDR=(RDAX*256);
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4)))
    {}
    _temp=SSPDR&0xff;
//    /* Write data out */
//    SSPDR=0xff;  //dummy byte
//    /* Wait for transfer to be completed */
//    while((SSPSR & (1<<4))) // receive data
//    {}
//    _temp=SSPDR&0xff;
    
    SSPDR=0xff;  //dummy byte
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4))) // receive data
    {}
    _temp1=SSPDR;
    _result=_temp*8+_temp1/32;
    P0SET = 1<<20;
    return _temp;
}

extern "C" int spi_receive_y (void)
{
    P0CLR = 1<<20;
    int _result=0;
    int _temp=0,_temp1=0;
   // LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);

    SSPDR=(char)RDAY;
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4)))
    {}
  //  P0SET = 1<<20;
  //  P0CLR = 1<<20;

    /* Write data out */
    SSPDR=0xff;  //dummy byte
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4))) // receive data
    {}
    _temp=SSPDR&0xff;
    //P0SET = 1<<20;
    //P0CLR = 1<<20;

    SSPDR=0xff;  //dummy byte
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4))) // receive data
    {}
    _temp1=SSPDR&0xff;
    _result=_temp*8+_temp1/32;
    P0SET = 1<<20;
    return _result;
}


extern "C" void initTimer1( void )
{
    PINSEL1 |= (3<<10);           // enable cap1.3 on p0.21 (pin P6 on logomatic v2)

    T1TCR = (1<<1);                             // disable and reset Timer0
    T1CTCR = (3<<2)|(3<<0);                     // count using CAP1.3 as the counter
    T1TCR = 1;                                  // enable the timer
}

#if LOG_DATA
extern "C" void card_init( void )
{
	if(!sd_raw_init()) //Attempt to init SD raw mode
                while (1);

	if(openroot()) //Attempt to open root directory
		while (1);
}

extern "C" void log_string( char *buf )
{
	int stringSize = strlen(buf);
	fat16_write_file(handle, (unsigned char*)buf, stringSize);
	sd_raw_sync();
}

void LoggingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    int count = 0;
    sprintf(name,"TCK%02d.csv",count);
    while(root_file_exists(name))
    {
        count++;
        sprintf(name,"TCK%02d.csv",count);
    }
    handle = root_open_new(name);  
    sd_raw_sync();    
    log_string((char *)"speed,setpoint,curr_prop,speedprop,push_start_delay,filtered_vals\n");

    while(true)
    {
        sprintf(_str1,"%0.3f,%0.3f,%0.3f,%0.3f,%d,%0.3f\n",_speed,PIDArgs.setpoint,curr_prop,speed_prop,push_start_delay,filtered_vals[BUFFER_LENGTH -1]);
        log_string(_str1);
        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}
#endif

void readSwitches( void )
{
    int pinval = P0PIN;
    if(pinval&(1<<10))
      {
        sw_mode=sw_press;
      }
      else
      {
        sw_mode=sw_release;
      }


    if(button_longhold>0)
    {
    button_longhold--;
    }

    if((sw_mode==sw_press) && (sw_pre_mode==sw_release))  // switch pressed
    {
      pressed_delay=0;
      cruse=0;
    }
    if((sw_mode==sw_press) && (sw_pre_mode==sw_press))  // switch hold  
    {
      pressed_delay++;
    }
    if((sw_mode==sw_release) && (sw_pre_mode==sw_press)) // releasing
    {   
      
       if(pressed_delay<7 && pressed_delay>0)
            { pressed_delay=0;
              if(button_longhold>0)   //quick press within 3s after long hold
              {
                cruse=1;
              }
              else
              {
                
                if(DRIVE_MODE==MANUAL_MODE)
                    {
                    DRIVE_MODE = AUTO_MODE;
  //                  LogomaticV2Stat(0, LOGOMATIC_STAT_OFF);
                    P0CLR = 1<<GREEN_LED;
                    }
                else
                    {
                    DRIVE_MODE = MANUAL_MODE;
  //                  LogomaticV2Stat(0, LOGOMATIC_STAT_ON);
                    P0SET = 1<<GREEN_LED;
                    } 
              }
            }
            else
            {
            sw_state=sw_coast;
            button_longhold=30;
            }
    }
    sw_pre_mode=sw_mode;

}
void WDT_init(void)
{
  #define WDT_COUNT_54p6_us 0xF00D
  #define WDT_COUNT_1p0_ms 0x4650 // (1ms)*(72MHz) / 4
  #define WDT_COUNT_1p5_ms 0x6978 // (1p5ms)*(72MHz) / 4
  #define WDT_COUNT_2_ms 0x8ca0 // (1p5ms)*(72MHz) / 4
  #define WDT_COUNT_10_ms 0x2BF20 // (10ms)*(72MHz) / 4
  #define WDT_COUNT_20_ms 0x57E40 // (20ms)*(72MHz) / 4
  #define WDT_COUNT_MAX 0xFFFFFFFF
  WDTC = WDT_COUNT_2_ms;//0xFFFFFFFF;//WDT_COUNT_10_ms;
  WDMOD = 1<<0 | 1<<1; 
  WDFEED = 0xAA;
  WDFEED = 0x55;
}
void WDT_service(void)
{
  WDFEED = 0xAA;
  WDFEED = 0x55;
}
void Timer_1msTask(void *pvParameters)
{
  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
  WDT_init();
  while(1) 
  {
    WDT_service();
    vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
  }
}

void CountingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    float temp_accel,temp_dd;
    float New_SetPoint = 0.0f;

    int bat_cap;
    int prev_coast_time;
    int push_delay=0;
    int uart1_data=0;
    char msg[15];
    int crc;


////    for (int i =0;i<BUFFER_LENGTH;i++)
////    {
////        accel_vals[i]=0;
////        filtered_vals[i] = 0;
////        filtered_DD[i]=0;
////        diff[i] = 0;
////        doublediff[i]=0;
////    }
////    for (int i =0;i<MESG_LENGTH;i++)
////    {
////        mesg0[i]=0x0c+i;
////        mesg1[i]=0x0c+i;
////    }
    
    while(true)
    {
      readSwitches();
      bat_cap=getADC1();
//      uart1_data=(DRIVE_MODE<<6)|(sw_mode<<7);
//      uart1_data=(sw_mode<<7)|(curse<<5);
      uart1_data=(sw_mode<<7)|(cruse<<5)|(DRIVE_MODE<<6);
//      uart1_tx(0x00);
      for (int i=0;i<6;i++)
      {
        msg[i]=acc_data[i];
        
      }
      for (int i=6;i<12;i++)
      {
        msg[i]=gyro_data[i-6];
          
      }
      msg[12]=uart1_data;
//      msg[12]=0;
      crc=CRC(msg,13);
      msg[13]=(crc>>8)&0xff;
      msg[14]=crc&0xff;
      uart1_tx_array(msg,15);
      uart1_tx(0xff);
      uart1_tx(0x00);
//      uart0_tx_array(msg,15);
      
//      uart1_tx_array(acc_data,6);
//      uart1_tx_array(gyro_data,6);
//      uart1_tx(01);
//      uart1_tx((1<<1));
//      uart1_tx((1<<2));
//      uart1_tx((1<<3));
//      uart1_tx((1<<4));
//      uart1_tx((1<<5));
//      uart1_tx((1<<6));

//      uart1_tx(0b1);
//      uart1_tx(0b11);
//      uart1_tx(0b111);
//      uart1_tx(0b1111);
//      uart1_tx(0b11111);
//      uart1_tx(0b111111);
//
//      uart1_tx(0b1);
//      uart1_tx(0b11);
//      uart1_tx(0b111);
//      uart1_tx(0b1111);
//      uart1_tx(0b11111);
//      uart1_tx(0b111111);
//      vTaskDelay(10); 
//      uart1_tx(uart1_data);
//      uart1_tx(0b1111111);
     
//      uart0_tx(0xff);
//      uart0_tx(0x00);
//      uart1_tx(0xff);
//      uart1_tx(0x00);


      vTaskDelay(configTICK_RATE_HZ /50);
    //    vTaskDelay(20);
    }
//    portTickType xLastWakeTime;
//    xLastWakeTime = xTaskGetTickCount();

////    while(true)
////    {        
////        prev_coast_time = COAST_TIME;
////
////        readSwitches();
////
////        if (COAST_TIME<prev_coast_time)   // turn off the motor if the coast switch decreased the coast time
////        {
////            push_detected = false;
////            PIDArgs.setpoint = 0.0f;
////        }
////        
////        temp_accel = 0.0f;
////        temp_dd = 0.0f;
////        for (int i=0;i<(BUFFER_LENGTH-1);i++)
////        {
////            diff[i] = diff[i+1];
////            doublediff[i] = doublediff[i+1];
////            filtered_vals[i] = filtered_vals[i+1];
////            filtered_DD[i] = filtered_DD[i+1];
////            temp_accel += diff[i];
////            temp_dd += doublediff[i];
////        }
////
////        prev_cap_val=cap_val;
////
////        portENTER_CRITICAL();
////        {
////            cap_val = T1TC;
////            T1TCR = (1<<1);                      // reset and disable Timer0
////            T1TCR = 1;                           // enable Timer0
////        }
////        portEXIT_CRITICAL();
////
////        diff[BUFFER_LENGTH -1] = cap_val - prev_cap_val;
////        doublediff[BUFFER_LENGTH -1] = diff[BUFFER_LENGTH-1] - diff[BUFFER_LENGTH-2];
////
////        filter_y=filter_b[0]*cap_val+filter_z[0];
////        filter_z[0]=filter_b[1]*cap_val+filter_z[1]-filter_a[1]*filter_y;
////        filter_z[1]=filter_b[2]*cap_val+filter_z[2]-filter_a[2]*filter_y;
////        filter_z[2]=filter_b[3]*cap_val+filter_z[3]-filter_a[3]*filter_y;
////        filter_z[3]=filter_b[4]*cap_val-filter_a[4]*filter_y;
////        
////        _speed = ( (float)cap_val * ((float)p_Args->frequency) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) );
////        filter_speed = ( (float)filter_y * ((float)p_Args->frequency) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) ); 
////        filtered_vals[BUFFER_LENGTH-1] = ((float)diff[BUFFER_LENGTH-1]+ temp_accel)/((float)BUFFER_LENGTH);
////        filtered_DD[BUFFER_LENGTH-1] = ((float)doublediff[BUFFER_LENGTH-1]+ temp_dd)/((float)BUFFER_LENGTH);
////          filter_acc=_speed-pre_speed;
////          pre_speed=_speed;
////          filter_dd=filter_acc-pre_acc;
////          pre_acc=filter_acc;
////
////
////        if ( DRIVE_MODE == AUTO_MODE )
////        {
////            if (DELAY_AFTER_MANUAL < AUTO_TIME_DELAY)   // FSR was pressed recently
////            {
////                //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_OFF);
////                DELAY_AFTER_MANUAL++;
////                New_SetPoint = 0.0f;
////                PIDArgs.setpoint=0.0f;
////            }
////            else    // if FSR has been released for a certain amount of time
////            {
////                if ( DRIVE_MODE == AUTO_MODE )   // if in Auto Mode and check for a push
////                {
////                   // LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);  
////                   if(PIDArgs.setpoint>0.0f)
////                   {
////                   }
////                   else
////                   {                   
////                     push_detected_delay++;
////                     if(filtered_vals[BUFFER_LENGTH -1]>16 && filtered_DD[BUFFER_LENGTH -1]>0 && filtered_vals[BUFFER_LENGTH -1]<300 && push_detected_delay>4)
////                     if(filter_acc>0.1f && filter_dd>0 && push_detected_delay>4)
////                      {
////                          push_detected = true;
////                          push_start_delay=0;
////                      }
////                   }
////                    if ( push_detected )
////                    {   
////                        push_start_delay++;
////                        if(push_start_delay>10)
////                        {
////                            push_detected = false;
////                            New_SetPoint =0.0f;
////                            //push_detected_delay=0;
////                        }
////                        else
////                        {
////                          if ( _speed < New_SetPoint )
////                          {
////                              if(push_start_delay>=0)
////                              {
////                                if(New_SetPoint>PIDArgs.setpoint*1.05)
////                                {
////                                  if(pip)
////                                  {
////                                    if(New_SetPoint>filter_speed*1.05)
////                                    {
////                                      PIDArgs.setpoint = New_SetPoint;
////                                      coasting_time=0;
////                                      push_detected = false;
////                                      New_SetPoint = 0.0f;
////                                    }
////                                    else
////                                    {
////                                    push_detected = false;
////                                    New_SetPoint =0.0f;
////                                    }
////                                  }
////                                  else
////                                  {
////                                    PIDArgs.setpoint = New_SetPoint;
////                                    coasting_time=0;
////                                    push_detected = false;
////                                    New_SetPoint = 0.0f;
////                                  }
////  
////                                }
////                                else
////                                  {
////                                    push_detected = false;
////                                    New_SetPoint =0.0f;
////                                  }
////                              }
////                              else
////                              {
////                                push_detected = false;
////                                New_SetPoint =0.0f;
////                              }
////                            pip=false;
////                          }
////                          else
////                          {
////                              if(filtered_vals[BUFFER_LENGTH -1]<50)
////                              if(filtered_vals[BUFFER_LENGTH -1]<300)
////                              {
////                                New_SetPoint = _speed;
////                                if ( New_SetPoint < 1.0f )
////                                    New_SetPoint = 0;
////                              }
////                              else
////                              {
////                                  push_detected = false;
////                                  New_SetPoint =0.0f;
////                                  push_detected_delay=0;
////                                  pip=false;
////                              }
////                          }
////                        }
////                    }
////                }
////            }
////        }
////        if ( DRIVE_MODE == OFF_MODE )
////        {
////            //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
////            //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_OFF); 
////            New_SetPoint = 0.0f;
////            PIDArgs.setpoint = 0.0f;
////            coasting_time = 0;
////            push_detected = false;
////        }
////
////        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
////    }
  
}

void PIDTask( void * pvParameters )
{
    sPIDParams_t *  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);

    float Prev_SetPoint = 1.0f;

    float Prev_Integral = 0.0f;

    float stable_SetPoint = 1.0f;

    float stable_Integral = 2.3f;

    float pre_speed;
    int gyro_buff_size=10;
    int gyro_buff_index=0;
    signed int gyro_buff[gyro_buff_size];
    signed int gyro_sum=0;

        for (int i =0;i<data_length;i++)
    {
        acc_data_sum[i]=0;
//        acc_data[i]=0;
        acc_data_mean[i]=0;
        gyro_data_sum[i]=0;
        gyro_data[i]=0;
        gyro_data_mean[i]=0;
    
    }

    for (int i =0;i<gyro_buff_size;i++)
    {
        gyro_buff[i]=0;
    }
    
//    float acc_x=0.0f;


    i2c_gyro_init();
        delay_ms(1);
    i2c_acc_init();
      delay_ms(1);


    while(true)
    {



    get_gyro_data();
    vTaskDelay(2);
    get_acc_data();
    vTaskDelay(2);
    gyro_buff[gyro_buff_index]=gyro_data[0]+gyro_data[1]*256;
    gyro_buff[gyro_buff_index]=gyro_buff[gyro_buff_index]&32767-gyro_buff[gyro_buff_index]&32768;
    gyro_buff[gyro_buff_index]=gyro_buff[gyro_buff_index]>500 ? gyro_buff[gyro_buff_index]:0;
    gyro_buff[gyro_buff_index]=gyro_buff[gyro_buff_index]<-500 ? gyro_buff[gyro_buff_index]:0;
    gyro_buff_index=gyro_buff_index>=9 ? 0:gyro_buff_index++;
    gyro_sum=0;
    for (int i =0;i<gyro_buff_size;i++)
    {
       gyro_sum=gyro_sum+ gyro_buff[i];
    }
    
    if (gyro_sum<-2500)
      {
      
      
      }


//    
////    vTaskDelay(1);
//    data_cal();
//    acc_data_sum[0]=acc_data_sum[0]+acc_x;
//    acc_data_sum[1]=acc_data_sum[1]+acc_y;
//    acc_data_sum[2]=acc_data_sum[2]+acc_z;
//    gyro_data_sum[0]=gyro_data_sum[0]+gyro_x;
//    gyro_data_sum[1]=gyro_data_sum[1]+gyro_y;
//    gyro_data_sum[2]=gyro_data_sum[2]+gyro_z;
////    for (int i =0;i<data_length;i++)
////    {
////        acc_data_sum[i]=acc_data_sum[i]+acc_data[i];
////        gyro_data_sum[i]=gyro_data_sum[i]+gyro_data[i];
////    
////    }
//    data_count++;
//    if(data_count==10)
//    {   
//      acc_data_mean[0]=(acc_data_sum[0]/10)%256;
//      acc_data_mean[1]=(acc_data_sum[0]/10)/256;
//      acc_data_mean[2]=(acc_data_sum[1]/10)%256;
//      acc_data_mean[3]=(acc_data_sum[1]/10)/256;
//      acc_data_mean[4]=(acc_data_sum[2]/10)%256;
//      acc_data_mean[5]=(acc_data_sum[3]/10)/256;
//      gyro_data_mean[0]=(gyro_data_sum[0]/10)%256;
//      gyro_data_mean[1]=(gyro_data_sum[0]/10)/256;
//      gyro_data_mean[2]=(gyro_data_sum[1]/10)%256;
//      gyro_data_mean[3]=(gyro_data_sum[1]/10)/256;
//      gyro_data_mean[4]=(gyro_data_sum[2]/10)%256;
//      gyro_data_mean[5]=(gyro_data_sum[2]/10)/256;
//
//       for (int i =0;i<data_length;i++)
//      {
////          acc_data_mean[i]=acc_data_sum[i]/10;
////          gyro_data_mean[i]=gyro_data_sum[i]/10;
//          acc_data_sum[i]=0;
//          gyro_data_sum[i]=0;
//      
//      } 
//    data_count=0;
//    }

    vTaskDelay(configTICK_RATE_HZ /40);
        //pre_adcval=_adcval;
//        _adcval = getADC();
        //spi_trans(RDAX);
//        tilt_x=spi_sw_receive_x();
        //P0SET = 1<<20;
       // P0CLR = 1<<20;
//       _wait_1us();
       // P0SET = 1<<20;
        //spi_trans(RDAY);
//        tilt_y=spi_sw_receive_y();

        
//        i2c_gyro_init();
//        get_acc_data();
//        delay_ms(1);
          
//        delay_ms(1);
//          get_gyro_data();
//        delay_ms(1);
//        i2c_acc_init();
//        delay_ms(1);
//        get_acc_data();
        
        
//        acc_x=acc_test();
//        uart0_tx(0xff);
//        uart0_tx_array(gyro_data,data_length);
//        uart0_tx_array(acc_data,6);

//////        if ( DRIVE_MODE != OFF_MODE )
//////        {
//////            
//////            if ( sw_mode ==sw_press )
//////            {
//////                push_detected = false;
//////                PIDArgs.setpoint = 0.0f;
//////                DELAY_AFTER_MANUAL = 0;
//////               // if(_speed<1.5f)
//////                //{
//////                if( pressed_delay>2)
//////                {
//////                  manual_output= manual_output>623 ? manual_output : (manual_output+50);
////////                  if(manual_output+manual_step+(_speed)*(_speed)*(_speed)*10<_adcval)
////////                  {
////////                    manual_output=manual_output+manual_step+(_speed)*(_speed)*(_speed)*10;
////////                  }
////////                  else
////////                    manual_output=_adcval;
//////                }
//////                else
//////                  manual_output=0;  
//////
//////                dac_output = manual_output+400;
//////                if (dac_output>1023)
//////                    dac_output=1023;
//////                if (dac_output==400)
//////                    dac_output=0;
//////                DACR = (1<<16)|((dac_output)<<6);
////////                DACR = (1<<16)|((800)<<6);
//////
//////                speed_prop_sw= manual_output/10.0f/p_Args->Ki/_speed;
////////                }
////////                else
////////                {
////////                DACR = (1<<16)|(0<<6);
////////                }
//////                //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
//////                //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);  
//////            }
//////            else
//////            {   
////////                if(sw_state==sw_coast)
////////                {     
////////                      curr_prop_sw = manual_output/10.0f/p_Args->Ki/_speed;
////////                 if  ( ( curr_prop_sw > (speed_prop_sw*BRAKE_PROP) ) )
////////                      {
////////                          DACR = (1<<16)|(0<<6);
////////                          push_detected = false;
////////                          PIDArgs.setpoint = 0.0f;
////////                          stable=false;
////////                          sw_state=sw_stop;
////////                      }
////////                      else
////////                      {   
////////                         dac_output = manual_output+400;
////////                        if (dac_output>1023)
////////                            dac_output=1023;
////////                        if (dac_output==400)
////////                            dac_output=0;
////////                        DACR = (1<<16)|((dac_output)<<6);
////////                      }
////////                }
////////                else
//////                 {
//////                    manual_output=0;
//////                    if ( Prev_SetPoint != p_Args->setpoint )
//////                    {
//////                        Prev_SetPoint = p_Args->setpoint;
//////                        duty = 0;
//////                        output = 0;
//////    
//////                        prev_prop  = curr_prop;
//////    
//////                        stable=false;
//////                        pid_delay=0;
//////                        duty_stable=0;
//////                    }
//////                    if ( p_Args->setpoint > 0.0f )
//////                    {
//////                        curr_prop = output/10.0f/p_Args->Ki/_speed;
//////                        ratio=curr_prop;
//////                        log_speed=_speed;
//////                        pid_delay++;
//////                        if ( !stable )
//////                        {
//////                            output = (p_Args->Ki * 1.8*p_Args->setpoint);
//////                            output *= 10.0f;
//////                                duty = (output>0.0f) ? ((int)output) : ((int)0);
//////                                duty = (duty<(1023-400)) ? duty : (1023-400);
//////                                duty += 400;
//////                                if( duty<=400)
//////                                    duty=0;                         
//////                                DACR = (1<<16)|(duty<<6);
//////                                
//////                                coasting_time=0;
//////                                speed_prop = curr_prop;
//////                                if ( speed_prop > 3.3f ) speed_prop = 3.3f;
//////                                if ( curr_prop > 4.0 )
//////                                {
//////                                    DACR = (1<<16)|(0<<6);
//////                                    push_detected = false;
//////                                    PIDArgs.setpoint = 0.0f;
//////                                    stable=false;
//////                                }
//////                        }
//////                        if(pid_delay==7) 
//////                        {
//////                            duty_stable=duty;
//////                            stable=true;  
//////                        }
//////                        if(pid_delay==12) 
//////                            {
//////                              speed_prop = curr_prop;
//////                               if ( speed_prop > 3.3f ) speed_prop = 3.3f;
//////                            }
//////                        if ( stable )
//////                        {
//////                            if( curr_prop < (speed_prop*PUSH_PROP) && prev_prop < (speed_prop*PUSH_PROP)&& diff[BUFFER_LENGTH -1]<300 &&  push_detected_delay>4)   // (curr_prop-prev_prop)<-0.1f
//////                            {
//////                                push_detected = true;
//////                                push_start_delay=0;
//////                                pip=true;
//////                            }
//////                            prev_prop = curr_prop;
//////                            if  ( ( curr_prop > (speed_prop*BRAKE_PROP) ) )
//////                            {
//////                                DACR = (1<<16)|(0<<6);
//////                                push_detected = false;
//////                                PIDArgs.setpoint = 0.0f;
//////                                stable=false;
//////                            }
//////                            else
//////                                DACR = (1<<16)|(duty_stable<<6);
//////                        }
//////                    }
//////                    else
//////                        DACR = (1<<16)|(0<<6);
//////                }   
//////            }
//////        }
//////        else
//////        {
//////            DACR = (1<<16)|(0<<6);
//////            push_detected = false;
//////            PIDArgs.setpoint = 0.0f;
//////            stable=false;
//////        }
//////        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}

int main( void )
{
   
   
   // Initialize the logomatic board processor and preipherals
   bootup();
   initADC();
//   spi_sw_init();
   initTimer1();
    //spi_trans(MEAS);
   // Create the Logging task
   uart_init();
   i2c_init();
   
//   i2c_acc_init();
//   delay_ms(1000);
//   i2c_gyro_init();
   xTaskCreate(CountingTask,
               reinterpret_cast<const signed char *>("TCK"),
               500,
               &CountingArgs,
               COUNTING_TASK_PRIORITY,
               &xCountingTaskHandle
               );
   xTaskCreate(Timer_1msTask,
               reinterpret_cast<const signed char *>("Timer1ms"),
               TASK_STACK_SIZE,
               &Timer1msArgs,
               Timer_1ms_TASK_PRIORITY,
               &xTimer_1msHandle
             );

   // Create the PID controller task
   xTaskCreate(PIDTask,
               reinterpret_cast<const signed char *>("PID"),
               TASK_STACK_SIZE,
               &PIDArgs,
               PID_TASK_PRIORITY,
               &xPIDTaskHandle
               );

#if LOG_DATA
   card_init();
   // Create the Logging task
     xTaskCreate(LoggingTask,
                 reinterpret_cast<const signed char *>("LOG"),
                 TASK_STACK_SIZE,
                 &LoggingArgs,
                 LOGGING_TASK_PRIORITY,
                 &xLoggingTaskHandle
               );
#endif
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
    //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
   
   // Start the multitasking environment
   //P0CLR = 1<<GREEN_LED;
   vTaskStartScheduler();
   
   return 0;
} // main

static void I2CISR (void) __attribute__ ((naked));
static void I2CISR (void)
{
portSAVE_CONTEXT ();
I2CISR_handler();
portRESTORE_CONTEXT();
}

void data_cal(void)
{
    acc_x=(  acc_data[0]&0b00001111+acc_data[1]*256 );
    acc_y=(  acc_data[2]&0b00001111+acc_data[3]*256 );
    acc_z=(  acc_data[4]&0b00001111+acc_data[5]*256 );
    gyro_x=( gyro_data[0]+gyro_data[1]*256 );
    gyro_y=(  gyro_data[2]+gyro_data[3]*256 );
    gyro_z=(  gyro_data[4]+gyro_data[5]*256 );
}
float acc_cal_x(void)
  {
    acc_x=(  acc_data[0]&0b00001111+acc_data[1]*256 )/4096.0 *4.0-2;
    return acc_x;
  }
float acc_cal_y(void)
  {
    acc_y=(  acc_data[2]&0b00001111+acc_data[3]*256 )/4096.0 *4.0-2;
    return acc_y;
  }
float acc_cal_z(void)
  {
    acc_z=(  acc_data[4]&0b00001111+acc_data[5]*256 )/4096.0 *4.0-2;
    return acc_z;
  }

void i2c_init(void)
{
  PINSEL0=(PINSEL0 & 0xffffff0f) | 0x50;
  I20SCLH = 200;
  I20SCLL = 200;     //PCLK=12MX6    FRQ=PCLK/(100+100)=360KHZ
 // ISR SETTINGS
   VICIntSelect = 0x00;  //I2C0 
   VICVectCntl1 = 0x20 | 9;    //I2C0
   VICVectAddr1 = ( long )I2CISR;
   VICIntEnable |= ( 1<<9 );   // ISR ENABLE
   VICDefVectAddr=( long )I2CISR;
   PCONP|=1<<7;// I2C POWER ENABLE
   I20ADR=0X72;
   I20CONCLR = 0x6c;
   I20CONSET = 0x40;  //ENABLE
   i2c_bus_busy=0; // i2c bus is not busy
}

void i2c_acc_init(void)
{
  int _temp_i=0;
//  I2CAddress = gyro_add;
//  while(i2c_bus_busy)
//  {_temp_i++;}
  i2c_bus_busy=1;    // i2c bus is busy
  I2CAddress = acc_add;
  I2CData = acc_init;
  I2C_reg_Address=CTRL_REG1;
  write_count=1;
//  i2cact=write;
  multi_read=0;
  I20CONCLR = 0x6c;
  I20CONSET = 0x40;  //ENABLE
  I20CONSET = 0x64; //start
//  delay_ms(1000);
//  while(i2c_bus_busy)
////  {_temp_i++;}
//  i2c_bus_busy=1;    // i2c bus is busy
//  I2CAddress = acc_add;
//  I2CData = 0x07;
//  I2C_reg_Address= ACC_XYZ_DATA_CFG ;
//  write_count=1;
//  i2cact=write;
//  multi_read=0;
//  I20CONCLR = 0x6c;
//  I20CONSET = 0x40;  //ENABLE
//  I20CONSET = 0x64; //start
//  delay_ms(1000);
}

void i2c_gyro_init(void)
{
  int _temp_i=0;
//    while(true)
//  {if (i2c_bus_busy==0)
//  break;}
  i2c_bus_busy=1;    // i2c bus is busy
  I2CAddress = gyro_add;
  I2CData = 0b10001111;
  I2C_reg_Address=0x20;
  write_count=1;
//  i2cact=write;
  multi_read=0;
  I20CONCLR = 0x6c;
  I20CONSET = 0x40;  //ENABLE
  I20CONSET = 0x64; //start
}

void get_acc_data(void)
{
  I2CAddress = acc_add;
  I2C_reg_Address = OUT_X_LSB;
  i2cact=read_acc;
  multi_read=1;
  read_count=0;

  I20CONCLR = 0X6C;

//  delay_ms(1);
  I20CONSET = 0x00000040;   //enable
  I20CONSET = 0x00000020;   //start
}
void get_gyro_data(void)
{
  I2CAddress = gyro_add;
  I2C_reg_Address =OUT_X_GYRO ;
  i2cact=read_gyro;
  multi_read=1;
  read_count=0;

  I20CONCLR = 0X6C;

//  delay_ms(1);
  I20CONSET = 0x00000040;   //enable
  I20CONSET = 0x00000020;   //start
}

void I2CISR_handler (void)
{
  switch (I20STAT&0xf8)
  {
//    case (0x00):   //bus error
//      I20CONSET =0X14;
//    break;
    case (0x08):   //start transmitted
      {
//      I20CONCLR=0x20;  // clear sta
//        I20DAT=gyro_add;
//      if (i2cact)     //read
//        {
//          I20DAT=I2CAddress|1; 
//        }
//        else    //write
//        {
          I20DAT=I2CAddress; 
//        }  
      I20CONCLR=0X20;  //CLEAR Sta
      
      }
     break;
    case (0x10):  // repeat start transmitted
     
      I20DAT=I2CAddress|1; ;  //read
      I20CONCLR=0x20;  // clear start & SI
    break;
    case (0x18):
      {
//      if (i2cact)     //read  write reg_address first
//        {
//          I20DAT=I2C_reg_Address; 
//        }
//        else    //write reg_add
//        {
          I20DAT=I2C_reg_Address; 
//        }  
      }
    break; 
//    case (0x20):
//   // I20DAT = I2CAddress;
////    acc_data[1]=0x23;
//    
//    break;
    
    case (0x28):
//      acc_data[1]=0x23;
      if(multi_read==0)   //not multi read
      {
        if (write_count>0)
        {
        I20DAT=I2CData;    //write acc_init to acc sensor
        write_count--;
        }
        else
        {
//        acc_data[1]=0x23;
        
        I20CONSET = 0x10;  //stop will transmitted
        i2c_bus_busy=0;  // free i2c bus
        }
      }
      else
      {
        I20CONSET = 0x00000020; // repeat start
      
      }   
    break;
    case (0x40):
     I20CONSET = 1<<2; // AA=1 ACK will be returned
    
    break;
    case (0x50):
      if(read_count<data_length)
      {
        if(i2cact==read_acc)
        {
        acc_data[read_count]=I20DAT;
        }
        else
        {
        gyro_data[read_count]=I20DAT;  
        }

        if(read_count==(data_length-2))
        {
          I20CONCLR= 1<<2; // AA=0 
        }
        read_count++;
      }
      else
      {
       
      }
    
    break;
    
    case (0x58):
           if(i2cact==read_acc)
          {
          acc_data[read_count]=I20DAT;
          }
          else
          {
          gyro_data[read_count]=I20DAT;  
          }
               
         I20CONSET = 0x10;  //stop will transmitted
         i2c_bus_busy=0; // free i2c bus
            
    break;
    
    
    default:
    {
      I20CONSET = 0x10;  //stop will transmitted
      i2c_bus_busy=0; // free i2c bus
//      acc_data[1]=0x23;
    }
    break;
  }

    I20CONCLR = 0X08;
    VICVectAddr=0x00;
   
}
